# -*- coding = utf-8 -*-
# @Time : 2023/9/12 9:36
# @Author : 龙王赘婿_彪少
# @File : PTC200.py
# @Software: PyCharm

# 功能：PTC200 测试encoder程序


from socket import *
from threading import Thread
import struct
import time
import binascii
from datetime import datetime, timedelta

class MYTCP:
    current_time = datetime.now()
    last_time = datetime.now()
    last_encoder = datetime.now()

    count = 0

    def __init__(self):
        self.ip = "192.168.8.31"
        # self.ip = "192.168.111.250"
        self.port = 9000
        self.client_socket = socket(AF_INET, SOCK_STREAM)
        self.client_socket.connect((self.ip, self.port))
        print("connected")
        print(datetime.now())
        t1 = Thread(target=self.recv_thread)
        t2 = Thread(target=self.send_thread)
        t1.start()
        t2.start()
        t1.join()
        t2.join()
        self.client_socket.close()

    # 接收线程
    def recv_thread(self):
        while True:
            recv_data = self.client_socket.recv(1024)  # 最大接收1024字节
            # self.print_recv_data(recv_data)
            # self.check_encoder(recv_data)
            self.check_encoder3(recv_data)

            self.current_time = datetime.now()
            if self.current_time - self.last_time > timedelta(seconds=5):
                print(self.current_time - self.last_time)
                print("*"*100)
            self.last_time = self.current_time
            # self.decode_recv_data(recv_data)

    @staticmethod
    def print_recv_data(raw_data):
        print(datetime.now(), f"recv：len = {len(raw_data)} data =", end=" ")
        for i in range(len(raw_data)):
            print(f"{raw_data[i]:02x}", end=" ")
        print()

    # 解析编码器和速度报文
    @staticmethod
    def decode_recv_data(raw_data: bytes):
        # 测试接收：01 06 60 40 00 01 57 de
        s = struct.Struct("<2B2H1I8f4i1H2B")
        up = s.unpack(raw_data)
        head = up[0]
        version = up[1]
        type = up[2]
        len = up[3]
        count = up[4]

        speed_lf = up[5]
        speed_rf = up[6]
        speed_lb = up[7]
        speed_rb = up[8]

        degrees_lf = up[9]
        degrees_rf = up[10]
        degrees_lb = up[11]
        degrees_rb = up[12]

        line_lf = up[13]
        line_rf = up[14]
        line_lb = up[15]
        line_rb = up[16]

        crc = up[17]
        end1 = up[18]
        end2 = up[19]

        # print(f"head = {head:02x} version = {version:02x} type = {type:04x}")
        # print(f"speed: lf = {speed_lf:f} rf = {speed_rf:f} lb = {speed_lb:f} rb = {speed_rb:f}")
        # print(f"degree: lf = {degrees_lf:f} rf = {degrees_rf:f} lb = {degrees_lb:f} rb = {degrees_rb:f}")
        # print(f"line: lf = {line_lf:d} rf = {line_rf:d} lb = {line_lb:d} rb = {line_rb:d}")

    def ackermann(self):
        mode = 1
        v = -0.5
        w = 0.5
        r = 0
        deg = 0
        buf = [0xff, 0xfe, 0x11, 31, 0x00, mode, v, w, r, deg]
        s = struct.Struct("<2B2H1I1B4f")
        packed_data = s.pack(*buf)
        crc = self.crc16_cal(packed_data)
        crc_pack = struct.Struct('H').pack(crc)
        end = [0xfd, 0xfa]
        end_pack = struct.Struct("2B").pack(*end)
        packed_data = packed_data + crc_pack + end_pack
        return packed_data

    @staticmethod
    def crc16_cal(datalist):
        # from "https://code84.com/461550.html"
        test_crc = 0xFFFF
        poly = 0xa001
        # poly=0x8005
        numl = len(datalist)
        for num in range(numl):
            data = datalist[num]
            test_crc = (data & 0xFF) ^ test_crc
            for bit in range(8):
                if (test_crc & 0x1) != 0:
                    test_crc >>= 1
                    test_crc ^= poly
                else:
                    test_crc >>= 1
        return test_crc

    # 发送线程
    def send_thread(self):
        while True:
            # 给服务端发消息
            # send_data = self.ackermann()
            # n2 = binascii.b2a_hex(send_data)
            # print(f"n2={n2},type={type(n2)}")
            # self.client_socket.send(send_data)
            time.sleep(0.05)

    def check_encoder(self, raw_data : bytes):
        """
        检查编码器数据是否间隔大于0.5s，raw_data的头为ff fe 03 00
        :return:
        """

        if raw_data[0] != 0xff or raw_data[1] != 0xfe or raw_data[2] != 0x03 or raw_data[3] != 0x00:
            if datetime.now() - self.last_encoder > timedelta(seconds=0.5):
                print(f"encoder slow {datetime.now() - self.last_encoder}")
            self.last_encoder = datetime.now()
            return

    def check_encoder2(self, raw_data : bytes):
        """
        检查编码器数据是否间隔大于0.5s，raw_data的头为ff fe 03 00
        :return:
        """
        temp_time_now = datetime.now()
        print(temp_time_now, f"recv：len = {len(raw_data)} data =", end=" ")
        for i in range(len(raw_data)):
            print(f"{raw_data[i]:02x}", end=" ")
        print()

        if raw_data[0] != 0xff or raw_data[1] != 0xfe or raw_data[2] != 0x03 or raw_data[3] != 0x00:
            if temp_time_now - self.last_encoder > timedelta(seconds=0.5):
                print(temp_time_now, self.last_encoder, f"encoder slow {temp_time_now - self.last_encoder}")
            self.last_encoder = temp_time_now
            return

    def check_encoder3(self, raw_data : bytes):
        """
        检查编码器数据是否间隔大于0.5s，raw_data的头为ff fe 03 00
        :return:
        """
        temp_time_now = datetime.now()

        # print(temp_time_now, f"recv：len = {len(raw_data)} data =", end=" ")
        # for i in range(len(raw_data)):
        #     print(f"{raw_data[i]:02x}", end=" ")
        # print()


        if raw_data[0] == 0xff and raw_data[1] == 0xfe and raw_data[2] == 0x03 and raw_data[3] == 0x00:

            if self.count < 500:
                self.count = self.count + 1
            else:
                self.count = 0
                print(temp_time_now)

            if temp_time_now - self.last_encoder > timedelta(seconds=0.5):
                print(temp_time_now, self.last_encoder, f"encoder slow {temp_time_now - self.last_encoder}")
            self.last_encoder = temp_time_now
            return


if __name__ == "__main__":
    obj = MYTCP()
